/***************************************************************************
 *   Copyright (C) 2007 by Autostop team                                   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

// libc
#include <stdio.h>

// camera
#include "camera.h"

// opencv
#define HAVE_LRINT
#include <opencv/cxcore.h>
#include <opencv/highgui.h>
#include <opencv/cv.h>

// config

const char* CASCADE_FILE = "/autostop/share/opencv/haarcascades/haarcascade_frontalface_default.xml";
const int SCALE = 6;	// image scaling factor
#ifdef __arm__
const char* OUTPUT_FILE	= "/var/www/faces.png";
const char* TMP_FILE	= "/var/www/faces_tmp.png"; // NOTE: must be on same filesystem as OUTPUT_FILE
#else
const char* OUTPUT_FILE	= "/autostop/images/faces.png";
const char* TMP_FILE	= "/autostop/images/tmp_faces.png";
#endif

const int MIN_SIZE	= 160; // min detected object size - larger - faster detection
const char* LOGFILE = "/autostop/log/facedetector.log";


// ------------------------- main ---------------------
int main( int argc, char** argv )
{
	// open logfil;e
	FILE* logfile = fopen( LOGFILE, "w" );
	if ( ! logfile )
	{
		logfile = stderr;
	}
	
	fprintf( logfile, "Initializing face detector\n");
	fflush( logfile );
	
	
	// initialize camera
	if ( camera_init() != CAMERA_OK )
	{
		fprintf( logfile, "Could not initialize camera: %s\n", camera_error_message() );
		return 1;
	}
		
	// get some data
	int width = camera_image_width();
	int height = camera_image_height();
	
	// minimum detectable oject size
	CvSize minObjectSize = cvSize( MIN_SIZE / SCALE, MIN_SIZE / SCALE );
	
	// preare bufer for image
	uint8_t image[ width * height ];
	
	// create ipl images
	IplImage*	pImage = cvCreateImage
			( cvSize( width, height )
			, IPL_DEPTH_8U
			, 1 // one chanel
			);
	IplImage* pSmallImage = cvCreateImage
							( cvSize( width/SCALE, height/SCALE )
							, IPL_DEPTH_8U
							, 1
							);
	if ( ! pImage || ! pSmallImage )
	{
		fprintf( logfile, "could not allocte image\n");
		fflush( logfile );
		return 1;
	}
	
	// load cascade
	CvHaarClassifierCascade* pCascade = (CvHaarClassifierCascade*)cvLoad( CASCADE_FILE );
	if ( ! pCascade )
	{
		fprintf( logfile, "not load detection pattern from %s\n", CASCADE_FILE);
		fflush( logfile );
		return 1;
	}
	
	// init storage
	CvMemStorage* pStorage = cvCreateMemStorage( 0 );
			
	fprintf( logfile, "Starting detection\n");
	fflush( logfile );

	while( true ) // infinte loop
	{
		// obtain image from camera
		if ( camera_get_image( image, width * height, NULL ) != CAMERA_OK )
		{
			fprintf( logfile, "Could not read image from camera: %s\n", camera_error_message() );
			fflush( logfile );
			return 1;
		}
		
		fprintf( logfile, "   Image acquired from camera\n");
		fflush( logfile );
	
		// copy buffer to ipl image
		// TODO test how it would be without copyingm but with putting directly into buffer
		uint8_t* src = image;
		for( int y = 0; y < height ; y++ )
		{
			uint8_t* dst = (uint8_t*)(pImage->imageData + y * pImage->widthStep);
			
			for( int x = 0; x < width; x++ )
			{
				(*dst++) = (*src++);
			}
		}
		
		fprintf( logfile, "   Image copied to IPL data structure\n");
		fflush( logfile );
		
		// scale image
		//cvPyrDown( pImage, pSmallImage, CV_GAUSSIAN_5x5 ); // NOTE this supports only  by 2 scaling
		cvResize( pImage, pSmallImage );
		
		fprintf( logfile, "   Image scaled down\n");
		fflush( logfile );
		
		// look for faces
		CvSeq* pFaces = cvHaarDetectObjects
							( pSmallImage
							, pCascade
							, pStorage
							, 1.2
							, 2
							, CV_HAAR_DO_CANNY_PRUNING
							, minObjectSize
							);
		
		fprintf( logfile, "   Faces detected\n");
		fflush( logfile );
		
		// draw all the rectangles
		for( int i = 0; i < pFaces->total; i++ )
		{
			// extract the rectanlges only
			CvRect rect = *(CvRect*)cvGetSeqElem( pFaces, i );
			
			cvRectangle( pImage
						, cvPoint( rect.x * SCALE, rect.y * SCALE)
						, cvPoint( ( rect.x + rect.width ) * SCALE
						, ( rect.y + rect.height ) * SCALE)
						, CV_RGB(255,0,0)
						, 1
						);
		}
		
		fprintf( logfile, "   Rectagles painted\n");
		fflush( logfile );
		
		// write image to tmp file
		cvSaveImage( TMP_FILE, pImage );
		
		// move tmpfile to target file
		rename( TMP_FILE, OUTPUT_FILE );
		
		fprintf( logfile, "   Saved to %s\n", OUTPUT_FILE );
		fflush( logfile );
	}
	
	return 0;
}

// eif

